#define LOG_TAG "IPLM_HAL"

#include "Iplm.h"
#include <log/log.h>
#include <cutils/properties.h>
#include <algorithm>

namespace vendor {
namespace ecarx {
namespace xma {
namespace iplm {
namespace V1_0 {
namespace implementation {

//using vendor::ecarx::xma::automotive::vehicle::V2_0::VendorVehicleProperty;

using std::chrono::milliseconds;
using std::chrono::steady_clock;
using std::function;
using std::lock_guard;
using std::mutex;
using std::this_thread::sleep_for;
using std::unique_lock;


Iplm::Iplm():
    mCookie(0),
    mSocket(-1),
    mPrecfgServicesMap(0),
    mIplmActivityCount(0),
    mVFCRetriggerBulletin(0),
    mIsTerminating(false),
    mDeathRecipient(new HidlDeathRecipient(this)) {
    ALOGI("%s", __func__);
}

Iplm::~Iplm() {
    ALOGI("%s, cleanup ThreadPool, close socket %d", __func__, mSocket);
    mIsTerminating = true;

    if (mReceiveThread.joinable())
        mReceiveThread.join();

    if (mWorkerThread.joinable())
        mWorkerThread.join();

    if (mCallbackThread.joinable()) {
        mCallbackCond.notify_all();
        mCallbackThread.join();
    }

    mIplmResourceGroupMap.clear();
    mEcuBulletinBoard.clear();

    close(mSocket);
    ALOGI("%s, cleanup successfully", __func__);
}

/***************************************************************************/
/******************************* Public Methods ****************************/
/***************************************************************************/

bool Iplm::initialize() {
    ALOGI("IPLM %s", __func__);

    loadConfig();
    mVehicleHelper.init();
    mVehicleHelper.setDID();

    internalInit();
    return initTransport();
}

Return<StatusCode> Iplm::setPrecfgServiceStatus(IPLM_Preconfigured_Service service, ServiceStatus status) {
    lock_guard<std::mutex> lk(mMiscMx);

    ALOGI("%s, Preconfigured Service registerd %#x, %s, Status: %s",
        __func__, service, dumpPreconfigServiceName(service), dumpPreconfigServiceStatus(status));

    if (status == ServiceStatus::OnLine) {
        mPrecfgServicesMap |= (service & 0xFFFF);
    } else if (status == ServiceStatus::OffLine) {
        mPrecfgServicesMap &= ~(service & 0xFFFF);
    }

    return StatusCode::SUCCESS;
}

Return<StatusCode> Iplm::subscribe(IPLM_Preconfigured_Service service, IPLM_ResourceGroup_Index resourceGroup, const sp<IIplmCallback>& callback) {
    std::string key = std::to_string(static_cast<uint32_t>(service)) + std::to_string(static_cast<uint32_t>(resourceGroup));

    ALOGI("%s, Service %#x %s is subscribing for resoureGroup %s",
        __func__, service, dumpPreconfigServiceName(service), dumpResourceGroup(resourceGroup));

    lock_guard<std::mutex> lk(mResourceGroupMx);
    if (callback == nullptr) {
        ALOGE("%s, callback is null pointer ???", __func__);
        return StatusCode::FAILED;
    }

    auto it = mIplmResourceGroupMap.find(resourceGroup);
    if (it == mIplmResourceGroupMap.end()) {
        ALOGE("%s, unknown resourceGroup %#x", __func__, resourceGroup);
        return StatusCode::FAILED;
    }

    auto & iplm_rg = it->second;
    auto _it = iplm_rg.cookieMap.emplace(key, mCookie);
    if (_it.second == false) {
        ALOGE("%s, failed reason: there are cookie mapped to the same key %s", __func__, key.c_str());
        return StatusCode::FAILED;
    }

    auto ret = iplm_rg.callbacks.emplace(mCookie, callback);
    if (ret.second == false) {
        ALOGE("%s, failed reason: there are callback mapped to the same key %s", __func__, key.c_str());
        return StatusCode::FAILED;
    }

    callback->linkToDeath(mDeathRecipient, mCookie++);

    Callback_Element element_ = {
        .what = callback,
        .index = resourceGroup,
        .triggerIPCP = iplm_rg.isCanTriggerIPCP,
    };

    lock_guard<std::mutex> lk_callback(mCallbackMx);
    mCallbackQueue.emplace(element_);
    mCallbackCond.notify_one();

    return StatusCode::SUCCESS;

}

Return<StatusCode> Iplm::unsubscribe(IPLM_Preconfigured_Service service, IPLM_ResourceGroup_Index resourceGroup) {
    std::string key = std::to_string(static_cast<uint32_t>(service)) + std::to_string(static_cast<uint32_t>(resourceGroup));

    ALOGI("%s, %s is unsubscribing for resoureGroup %s[%#x]",
         __func__, dumpPreconfigServiceName(service), dumpResourceGroup(resourceGroup), resourceGroup);

    lock_guard<std::mutex> lk(mResourceGroupMx);
    auto it = mIplmResourceGroupMap.find(resourceGroup);
    if (it == mIplmResourceGroupMap.end()) {
        ALOGE("%s, unknown resourceGroup %#x", __func__, resourceGroup);
        return StatusCode::FAILED;
    }

    auto & iplm_rg = it->second;
    auto nest_it = iplm_rg.cookieMap.find(key);
    if (nest_it == iplm_rg.cookieMap.end()) {
        ALOGE("%s, does not found related key %s", __func__, key.c_str());
        return StatusCode::FAILED;
    }

    auto callback_it = iplm_rg.callbacks.find(nest_it->second);
    if (callback_it != iplm_rg.callbacks.end()) {
        auto callback = callback_it->second;
        callback->unlinkToDeath(mDeathRecipient);
        iplm_rg.callbacks.erase(callback_it);
    }

    iplm_rg.cookieMap.erase(nest_it);

    return StatusCode::SUCCESS;
}

Return<StatusCode> Iplm::RequestResourceGroupWithPriority(IPLM_ResourceGroup_Index resourceGroup, IPLM_RG_PRIO priority) {
    ALOGI("%s, %s with priority %d", __func__, dumpResourceGroup(resourceGroup), priority);

    buildIpActivity(resourceGroup, priority);
    return StatusCode::SUCCESS;
}

Return<StatusCode> Iplm::ReleaseResourceGroup(IPLM_ResourceGroup_Index resourceGroup, IPLM_RG_PRIO priority = IPLM_RG_PRIO::PRIO_NORM) {
    ALOGI("%s, %s with priority %d", __func__, dumpResourceGroup(resourceGroup), priority);

    releaseIpActivity(resourceGroup, priority);
    return StatusCode::SUCCESS;
}

Return<IPLM_RG_State> Iplm::getResourceGroupState(IPLM_ResourceGroup_Index resourceGroup) {
    ALOGI("%s %s", __func__, dumpResourceGroup(resourceGroup));

    auto it = mIplmResourceGroupMap.find(resourceGroup);
    if (it == mIplmResourceGroupMap.end()) {
        ALOGE("%s, unknown resourceGroup %#x", __func__, resourceGroup);
        return IPLM_RG_State::STATE_NONE;
    }

    return it->second.state;
}

/***************************************************************************/
/******************************* Private Methods ****************************/
/***************************************************************************/

void Iplm::loadConfig() {
    uint32_t iplm_rmt_ = 0;

    iplm_rmt_ = property_get_int32(kIplmRMT, DEFAULT_REQUEST_MONITORING_TIMEOUT);
    mResourceGroupIndicator = property_get_int32(kIplmIHUResouceGroupID, DEFAULT_IPLM_MEMBERS);

    mRequestMonitoringTimeout = iplm_rmt_ / DEFAULT_IPLM_STEP_PERIOD;  // uint 100ms
    mIplmActivityBulletin = 0;

    mIhuMark = mResourceGroupIndicator & 0x00FF;
    ALOGI("%s, ihu IPLM ResourceGroupIndicator is %#x", __func__, mIhuMark);
}

void Iplm::internalInit() {
    mARSFlagSet.reset();
    mPriorityHighSet.reset();
    mPowerState = IPLM_Power_State::IPLM_SET_POWER_NULL;

    mVehicleHelper.setPowerState(mPowerState);

    mIplmResourceGroupMap.clear();
    Iplm_resource_group iplm_rg;

    iplm_rg.isCanTriggerIPCP = false;
    iplm_rg.state = IPLM_RG_State::STATE_NONE;
    iplm_rg.priority = IPLM_RG_PRIO::PRIO_NORM;
    iplm_rg.online_nodes.clear();
    iplm_rg.cookieMap.clear();
    iplm_rg.callbacks.clear();

    if (mIhuMark & static_cast<uint8_t>(IPLM_ResourceGroup_Index::RG_1)) {
        iplm_rg.members.clear();
        iplm_rg.members.emplace_back(NODE_TCAM);
        iplm_rg.members.emplace_back(NODE_VGM);

        mIplmResourceGroupMap.emplace(IPLM_ResourceGroup_Index::RG_1, iplm_rg);
        ALOGI("%s, RG_1 member sizes %lu", __func__, iplm_rg.members.size());
    }

    if (mIhuMark & static_cast<uint8_t>(IPLM_ResourceGroup_Index::RG_2)) {
        iplm_rg.members.clear();
        iplm_rg.members.emplace_back(NODE_TCAM);
        iplm_rg.members.emplace_back(NODE_VGM);

        if (mVehicleHelper.isDMMAvailable()) {
            iplm_rg.members.emplace_back(NODE_DMM);
        }

        mIplmResourceGroupMap.emplace(IPLM_ResourceGroup_Index::RG_2, iplm_rg);
        ALOGI("%s, RG_2 member sizes %lu", __func__, iplm_rg.members.size());
    }

    if (mIhuMark & static_cast<uint8_t>(IPLM_ResourceGroup_Index::RG_3)) {
        iplm_rg.members.clear();
        iplm_rg.members.emplace_back(NODE_TCAM);
        iplm_rg.members.emplace_back(NODE_VGM);

        mIplmResourceGroupMap.emplace(IPLM_ResourceGroup_Index::RG_3, iplm_rg);
        ALOGI("%s, RG_3 member sizes %lu", __func__, iplm_rg.members.size());
    }

    if (mIhuMark & static_cast<uint8_t>(IPLM_ResourceGroup_Index::RG_4)) {
        iplm_rg.members.clear();
        iplm_rg.members.emplace_back(NODE_TCAM);
        iplm_rg.members.emplace_back(NODE_VGM);

        if (mVehicleHelper.isDVRAvailable())
            iplm_rg.members.emplace_back(NODE_DVR);

        if (mVehicleHelper.isPASAvailable())
            iplm_rg.members.emplace_back(NODE_PAS);

        mIplmResourceGroupMap.emplace(IPLM_ResourceGroup_Index::RG_4, iplm_rg);
        ALOGI("%s, RG_4 member sizes %lu", __func__, iplm_rg.members.size());
    }

    if (mIhuMark & static_cast<uint8_t>(IPLM_ResourceGroup_Index::RG_5)) {
        iplm_rg.members.clear();
        iplm_rg.members.emplace_back(NODE_VGM);

        if (mVehicleHelper.isASDMAvailable())
            iplm_rg.members.emplace_back(NODE_ASDM);

        mIplmResourceGroupMap.emplace(IPLM_ResourceGroup_Index::RG_5, iplm_rg);
        ALOGI("%s, RG_5 member sizes %lu", __func__, iplm_rg.members.size());
    }

    if (mIhuMark & static_cast<uint8_t>(IPLM_ResourceGroup_Index::RG_6)) {
        iplm_rg.members.clear();
        iplm_rg.members.emplace_back(NODE_TCAM);
        iplm_rg.members.emplace_back(NODE_VGM);

        if (mVehicleHelper.isDMMAvailable())
            iplm_rg.members.emplace_back(NODE_DMM);

        if (mVehicleHelper.isDIMAvailable())
            iplm_rg.members.emplace_back(NODE_DIM);

        // Add PAS / DVR to ResourceGroup 6 as Geely Requirement change in 20R1 phase.
        if (mVehicleHelper.isDVRAvailable()) {
            iplm_rg.members.emplace_back(NODE_DVR);
        }

        if (mVehicleHelper.isPASAvailable()) {
            iplm_rg.members.emplace_back(NODE_PAS);
        }

        mIplmResourceGroupMap.emplace(IPLM_ResourceGroup_Index::RG_6, iplm_rg);
        ALOGI("%s, RG_6 member sizes %lu", __func__, iplm_rg.members.size());
    }

    if (mIhuMark & static_cast<uint8_t>(IPLM_ResourceGroup_Index::RG_7)) {
        // No requirements in SWR
        iplm_rg.members.clear();

        mIplmResourceGroupMap.emplace(IPLM_ResourceGroup_Index::RG_7, iplm_rg);
        ALOGI("%s, RG_7 member sizes %lu", __func__, iplm_rg.members.size());
    }

    mEcuBulletinBoard.clear();
    mEcuBulletinBoard.emplace(NODE_TCAM, mRequestMonitoringTimeout);
    mEcuBulletinBoard.emplace(NODE_VGM, mRequestMonitoringTimeout);

    if (mVehicleHelper.isASDMAvailable()) {
        ALOGI("%s, ASDMAvailable", __func__);
        mEcuBulletinBoard.emplace(NODE_ASDM, mRequestMonitoringTimeout);
    }
    if (mVehicleHelper.isDVRAvailable()) {
        ALOGI("%s, DVRAvailable", __func__);
        mEcuBulletinBoard.emplace(NODE_DVR, mRequestMonitoringTimeout);
    }
    if (mVehicleHelper.isPASAvailable()) {
        ALOGI("%s, PASAvailable", __func__);
        mEcuBulletinBoard.emplace(NODE_PAS, mRequestMonitoringTimeout);
    }
    if (mVehicleHelper.isDMMAvailable()) {
        ALOGI("%s, DMMAvailable", __func__);
        mEcuBulletinBoard.emplace(NODE_DMM, mRequestMonitoringTimeout);
    }
    if (mVehicleHelper.isDIMAvailable()) {
        ALOGI("%s, DIMAvailable", __func__);
        mEcuBulletinBoard.emplace(NODE_DIM, mRequestMonitoringTimeout);
    }

    // Default Ip activity message
    mIplmActivity.header.service_id = IP_ACTIVITY_SERVICE_ID;
    mIplmActivity.header.operation_id = IP_ACTIVITY_OPERATION_ID;
    mIplmActivity.header.op_type = IP_ACTIVITY_OPERATION_TYPE;
    mIplmActivity.header.protocol_version = IP_ACTIVITY_PROT_VER;
    mIplmActivity.header.payload_type = IP_ACTIVITY_DATA_TYPE;
    mIplmActivity.header.length = IP_ACTIVITY_LENGTH;
    mIplmActivity.header.proc = IP_ACTIVITY_PROC;
    mIplmActivity.header.senderHandleId = IP_ACTIVITY_SENDER_HANDLE_ID_BASE + 1;

    mIplmActivity.action = IPLM_AVAILABLE;
    mIplmActivity.prio = static_cast<uint8_t>(IPLM_RG_PRIO::PRIO_NORM);
    mIplmActivity.reserved = 0x0;

    // IHU request ResourceGroup1 as default action.
    buildIpActivity(IPLM_ResourceGroup_Index::RG_1, IPLM_RG_PRIO::PRIO_NORM);

    mIpAddrNodeMap.clear();
    mIpAddrNodeMap.emplace(PEER_TCAM_IP, NODE_TCAM);
    mIpAddrNodeMap.emplace(PEER_VGM_IP, NODE_VGM);
    mIpAddrNodeMap.emplace(PEER_DIM_IP, NODE_DIM);
    mIpAddrNodeMap.emplace(PEER_DMM_IP, NODE_DMM);
    mIpAddrNodeMap.emplace(PEER_DVR_IP, NODE_DVR);
    mIpAddrNodeMap.emplace(PEER_PAS_IP, NODE_PAS);
    mIpAddrNodeMap.emplace(PEER_ASDM_IP, NODE_ASDM);
}

bool Iplm::initTransport() {
    int32_t opt_val = 1;
    int32_t priority = IPLM_PCP;
    const char * eth0_str = "eth0";

    struct ifreq ifr;
    struct sockaddr_in broadcast_addr;
    struct sockaddr_in local_addr;
    struct sockaddr_in from_addr;

    memset(&broadcast_addr, 0, sizeof(broadcast_addr));
    memset(&local_addr, 0, sizeof(local_addr));
    memset(&from_addr, 0, sizeof(from_addr));
    memset(&ifr, 0, sizeof(ifr));

    strncpy(ifr.ifr_ifrn.ifrn_name, IFNAME, IFNAMSIZ);

    if((mSocket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
        ALOGE("%s: Failed to create socket error %s\n", __func__, strerror(errno));
        return false;
    }
    ALOGI("%s, created socket %d", __func__, mSocket);
    if(ioctl(mSocket, SIOCGIFBRDADDR, &ifr) == -1) {
        ALOGE("%s: Failed to ioctl socket %d error %s\n", __func__, mSocket, strerror(errno));
        close(mSocket);
        return false;
    }

    if(setsockopt(mSocket, SOL_SOCKET, SO_BROADCAST | SO_REUSEADDR, &opt_val, sizeof(opt_val)) < 0) {
        ALOGE("%s: Failed to setsockopt SO_BROADCAST socket %d error %s\n", __func__, mSocket, strerror(errno));
        close(mSocket);
        return false;
    }

    if(((strcmp(IFNAME, eth0_str)) != 0) && setsockopt(mSocket, SOL_SOCKET, SO_PRIORITY, &priority, sizeof (priority)) < 0) {
        ALOGE("%s: Failed to setsockopt SO_PRIORITY socket %d error %s\n", __func__, mSocket, strerror(errno));
    }

    if(setsockopt(mSocket , SOL_SOCKET, SO_BINDTODEVICE ,(char *)&ifr, sizeof(ifr)) < 0) {
        ALOGE("%s: Failed to setsockopt SO_BINDTODEVICE socket %d error %s\n", __func__, mSocket, strerror(errno));
        close(mSocket);
        return false;
    } else {
        ALOGI("%s: setsockopt IFNAME: %s VLAN PCP: %d\n", __func__, ifr.ifr_ifrn.ifrn_name, priority);
    }

    int default_ttl = 10;
    if(setsockopt(mSocket, IPPROTO_IP, IP_TTL, (char *) & default_ttl, sizeof (int)) < 0) {
        ALOGE("%s: Failed to set TTL as default %d ", __func__, default_ttl);
    } else {
        ALOGI("%s: set default TTL to %d", __func__, default_ttl);
    }

    memcpy(&broadcast_addr, &ifr.ifr_broadaddr, sizeof(ifr.ifr_broadaddr));
    broadcast_addr.sin_port = htons(PEER_BROADCAST_PORT);

    mBroadcastAddr = broadcast_addr;

    local_addr.sin_family = AF_INET;
    local_addr.sin_port = htons(LOCAL_BROADCAST_PORT);
    local_addr.sin_addr.s_addr = INADDR_ANY;

    if(bind(mSocket, (struct sockaddr *)&local_addr, sizeof(local_addr)) < 0) {
        ALOGE("%s: bind port:%d failure [%s] ", __func__, local_addr.sin_port, strerror(errno));
        close(mSocket);
        return false;
    }

    mReceiveThread = std::thread(&Iplm::doReceive, this);
    // mReceiveThread.detach();

    mWorkerThread = std::thread(&Iplm::iplmStep, this);
    // mWorkerThread.detach();

    mCallbackThread = std::thread(&Iplm::doCallback, this);
    return true;
}

void Iplm::doReceive() {
    int ret;
    int read_size = 0;
    struct sockaddr_in from_addr;
    socklen_t from_addr_len = sizeof(struct sockaddr);
    constexpr size_t iplm_size = sizeof(iplm_activity_t);
    uint8_t buf[256] = {0};
    iplm_activity_t * activity = reinterpret_cast<iplm_activity_t *>(buf);

    memset(&from_addr, 0, sizeof(from_addr));

    if (mSocket < 0) {
        ALOGE("%s, invalid socket fd, exit ...", __func__);
        return;
    }

    while (!mIsTerminating) {
        read_size = recvfrom(mSocket, buf, sizeof(buf), 0, reinterpret_cast<struct sockaddr *> (&from_addr), &from_addr_len);
        if (read_size < 0) {
            ALOGE("%s, recvfrom size < 0, retry again ~", __func__);
            continue;
        }

        auto rec_ip = std::string(inet_ntoa(from_addr.sin_addr));
        if( rec_ip.compare(LOCAL_AP_IP) == 0) {
            // ALOGD("%s, ignore local IPLM activity message", __func__);
            continue;
        }

        auto it = mIpAddrNodeMap.find(rec_ip);
        if (it == mIpAddrNodeMap.end()) {
            ALOGI("%s, ignore unknown ip address %s", __func__, rec_ip.c_str());
            continue;
        }

        auto node = it->second;
        //ALOGD("%s, recevied ip activity from ECU %s, ip %s", __func__, dumpECU_Name(node), rec_ip.c_str());
        // Reset ECU bulletin to default RMO value
        auto bulletin_it = mEcuBulletinBoard.find(node);
        if (bulletin_it != mEcuBulletinBoard.end()) {
            mBulletinMx.lock();
            bulletin_it->second = mRequestMonitoringTimeout;
            mBulletinMx.unlock();
        } else if (node & (NODE_ASDM | NODE_DVR | NODE_PAS | NODE_DMM | NODE_DIM)){
            ALOGD("%s, ECU %s not in mEcuBulletinBoard emplace it in", __func__, dumpECU_Name(node));
            mEcuBulletinBoard.emplace(node, mRequestMonitoringTimeout);
        }

        // Cleanup DTC
        if (mVehicleHelper.isUDPLinkLostDTCSet(node)) {
            ALOGI("%s, clean [%s] DTC", __func__, dumpECU_Name(node));
            mVehicleHelper.setUDPLinkLostDTC(node, IPLM_DTC_VALUE::CLEANUP_DTC);
        }

        // Sanity Check Received IP activity message.
        if (!sanityCheckIpActivity(activity)) {
            ALOGD("%s, ignore incompatible IP activity message", __func__);
            continue;
        }

        handleActiveRequest(activity, node);
    }
}

bool Iplm::sanityCheckIpActivity(iplm_activity_t * packet) {

    convertToLittleEndian(packet);

    // Check List
    if (packet->header.service_id != IP_ACTIVITY_SERVICE_ID ||
        packet->header.operation_id != IP_ACTIVITY_OPERATION_ID ||
        packet->header.op_type != IP_ACTIVITY_OPERATION_TYPE ||
        packet->header.payload_type != IP_ACTIVITY_DATA_TYPE ||
        packet->header.protocol_version != IP_ACTIVITY_PROT_VER ||
        packet->header.proc != IP_ACTIVITY_PROC ||
        packet->header.length != IP_ACTIVITY_LENGTH) {
            return false;
    }
    return true;
}

void Iplm::handleActiveRequest(const iplm_activity_t * packet, ECU_Node node) {
    uint8_t action = packet->action & (0xFF);
    uint8_t priority = packet->prio & (0xFF);
    uint8_t rg_index;
    IPLM_RG_State state;
    IPLM_Power_State powerState = IPLM_Power_State::IPLM_SET_POWER_NULL;

    //ALOGI("%s, node: %s, action: %#x, priority: %#x", __func__, dumpECU_Name(node), action, priority);
    if (!(action & IPLM_AVAILABLE)) {
        ALOGE("%s, invalid IP activity message (AVAILABLE bit is set to 0)", __func__);
        return;
    }

    for (auto rg_it = mIplmResourceGroupMap.begin(); rg_it != mIplmResourceGroupMap.end(); rg_it++) {
        rg_index = static_cast<uint8_t>(rg_it->first);
        // Check online nodes
        auto member_it = std::find(rg_it->second.members.begin(), rg_it->second.members.end(), node);
        if (member_it != rg_it->second.members.end()) {
            auto online_it = std::find(rg_it->second.online_nodes.begin(), rg_it->second.online_nodes.end(), node);
            if (online_it == rg_it->second.online_nodes.end()) {
                mResourceGroupMx.lock();
                rg_it->second.online_nodes.emplace_back(node);
                mResourceGroupMx.unlock();
            }
        } else {
           //ALOGE("%s, not found node %s in RG %#x", __func__, dumpECU_Name(node), rg_index);
           switch (rg_it->first) {
                case IPLM_ResourceGroup_Index::RG_1:
                    if(node & RG1_NODES) {
                        ALOGE("%s, not found node %s in RG1 add it in", __func__, dumpECU_Name(node));
                        rg_it->second.members.emplace_back(node);

                        mResourceGroupMx.lock();
                        rg_it->second.online_nodes.emplace_back(node);
                        mResourceGroupMx.unlock();
                    }
                    break;
                case IPLM_ResourceGroup_Index::RG_2:
                    if(node & RG2_NODES) {
                        ALOGE("%s, not found node %s in RG2 add it in", __func__, dumpECU_Name(node));
                        rg_it->second.members.emplace_back(node);

                        mResourceGroupMx.lock();
                        rg_it->second.online_nodes.emplace_back(node);
                        mResourceGroupMx.unlock();
                    }
                    break;
                case IPLM_ResourceGroup_Index::RG_3:
                    if(node & RG3_NODES) {
                        ALOGE("%s, not found node %s in RG3 add it in", __func__, dumpECU_Name(node));
                        rg_it->second.members.emplace_back(node);

                        mResourceGroupMx.lock();
                        rg_it->second.online_nodes.emplace_back(node);
                        mResourceGroupMx.unlock();
                    }
                    break;
                case IPLM_ResourceGroup_Index::RG_4:
                    if(node & RG4_NODES) {
                        ALOGE("%s, not found node %s in RG4 add it in", __func__, dumpECU_Name(node));
                        rg_it->second.members.emplace_back(node);

                        mResourceGroupMx.lock();
                        rg_it->second.online_nodes.emplace_back(node);
                        mResourceGroupMx.unlock();
                    }
                    break;
                case IPLM_ResourceGroup_Index::RG_5:
                    if(node & RG5_NODES) {
                        ALOGE("%s, not found node %s in RG5 add it in", __func__, dumpECU_Name(node));
                        rg_it->second.members.emplace_back(node);

                        mResourceGroupMx.lock();
                        rg_it->second.online_nodes.emplace_back(node);
                        mResourceGroupMx.unlock();
                    }
                    break;
                case IPLM_ResourceGroup_Index::RG_6:
                    if(node & RG6_NODES) {
                        ALOGE("%s, not found node %s in RG6 add it in", __func__, dumpECU_Name(node));
                        rg_it->second.members.emplace_back(node);

                        mResourceGroupMx.lock();
                        rg_it->second.online_nodes.emplace_back(node);
                        mResourceGroupMx.unlock();
                    }
                    break;
                case IPLM_ResourceGroup_Index::RG_7:
                    if(node & RG7_NODES) {
                        ALOGE("%s, not found node %s in RG7 add it in", __func__, dumpECU_Name(node));
                        rg_it->second.members.clear();

                        mResourceGroupMx.lock();
                        rg_it->second.online_nodes.clear();
                        mResourceGroupMx.unlock();
                    }
                    break;
                default:
                    ALOGE("%s, nothing todo", __func__);
                    break;
           }
        }

        if (action & rg_index) {
            ALOGD("%s, [IPLM Request] Active Request Session is triggerd for Resource Group %#x [%s], priority: %#x, by Node %s",
                     __func__, rg_index, dumpResourceGroup(rg_it->first), priority, dumpECU_Name(node));
        }
    }

    // IPLM Active Request Session
    mResourceGroupStateMx.lock();
    if (action & mIhuMark) {
        mARSFlagSet |= node;
        //ALOGI("%s, mARSFlagSet be set 1 by node: %s, mARSFlagSet: %#lx", __func__, dumpECU_Name(node), mARSFlagSet.to_ulong());
    } else {
        mARSFlagSet &= ~node;
        //ALOGI("%s, mARSFlagSet be set 0 by node: %s, mARSFlagSet: %#lx", __func__, dumpECU_Name(node), mARSFlagSet.to_ulong());
    }

    if (priority > 0) {
       mPriorityHighSet |= node;
    }

    if (priority == 0) {
        mPriorityHighSet &= ~node;
    }
    mResourceGroupStateMx.unlock();

    lock_guard<std::mutex> power_lk(mResourceGroupStateMx);
    // Update Power State
    if (mARSFlagSet.any()) {
        powerState = IPLM_Power_State::IPLM_SET_POWER_NORMAL;
    }
    // else {
    //      ALOGI("Not any active request session");
    //     // powerState = IPLM_Power_State::IPLM_SET_POWER_NULL;
    // }

    if (mPriorityHighSet.any()) {
        powerState = IPLM_Power_State::IPLM_SET_POWER_HIGH;
    }

    if (powerState != mPowerState) {
        mPowerState = powerState;
        ALOGI("%s, setPowerState: %d", __func__, mPowerState);
        mVehicleHelper.setPowerState(mPowerState);
    }
}

void Iplm::broadcastIpActivity() {
    uint8_t buf[IPLM_PACKET_SIZE] = {0};

    lock_guard<std::mutex> lk(mIplmActivityMx);
    mIplmActivityCount %= 0xFF;
    mIplmActivityCount++;
    mIplmActivity.header.senderHandleId = IP_ACTIVITY_SENDER_HANDLE_ID_BASE + mIplmActivityCount;

    memcpy(buf, &mIplmActivity, sizeof(mIplmActivity));
    convertToBigEndian(reinterpret_cast<iplm_activity_t *>(buf));

    //ALOGD("%s, broadcasting IPLM activity message", __func__);
    auto ret = sendto(mSocket, (uint8_t*)&buf, sizeof(buf), 0,
                    (struct sockaddr *) &mBroadcastAddr, sizeof(mBroadcastAddr));
    if (ret < 0) {
        ALOGE("%s, broadcast IPLM Activity failure...", __func__);
    }
}

void Iplm::buildIpActivity(const IPLM_ResourceGroup_Index rg, const IPLM_RG_PRIO priority) {
    lock_guard<std::mutex> lk(mIplmActivityMx);

    mIplmActivity.action |= (IPLM_AVAILABLE | static_cast<uint8_t>(rg));
    mIplmActivity.prio |= static_cast<uint8_t>(priority);
}

void Iplm::releaseIpActivity(const IPLM_ResourceGroup_Index rg, const IPLM_RG_PRIO priority) {
    lock_guard<std::mutex> lk(mIplmActivityMx);

    mIplmActivity.action &= ~(static_cast<uint8_t> (rg));
    mIplmActivity.prio &= ~(static_cast<uint8_t>(priority));
}

void Iplm::updateInternalResourceGroup() {

    bool isIPLinkReady = false;
    IPLM_RG_State previousState;
    IPLM_RG_State currentState = IPLM_RG_State::STATE_NONE;
    IPLM_Power_State powerState = mPowerState;

    lock_guard<std::mutex> lk_bulletin(mBulletinMx);
    for (auto it = mEcuBulletinBoard.begin(); it != mEcuBulletinBoard.end(); it++ ) {
        auto & bulletin = it->second;
        auto ecu_node = it->first;
        //ALOGD("%s, ECU node %s, bulletin %d", __func__, dumpECU_Name(it->first), it->second);
        if (bulletin > 0) {
            bulletin -- ;
        }

        // Timeout Case.
        if (bulletin == 0) {
            // Report DTC
            if(!mVehicleHelper.isUDPLinkLostDTCSet(ecu_node)) {
                mVehicleHelper.setUDPLinkLostDTC(it->first, IPLM_DTC_VALUE::REPORT_DTC);
            }
            // Reset Bulletin
            it->second = mRequestMonitoringTimeout;
            // Reset Priority BitSet for releasing Power State
            mResourceGroupStateMx.lock();
            mPriorityHighSet &= ~(ecu_node);
            // Clear ARS flag
            mARSFlagSet &= ~(ecu_node);
            mResourceGroupStateMx.unlock();

            lock_guard<std::mutex> lk2(mResourceGroupMx);
            for (auto rg_it = mIplmResourceGroupMap.begin(); rg_it != mIplmResourceGroupMap.end(); rg_it ++) {
                // Remove ECU_node from online vector.
                auto ret = std::find(rg_it->second.online_nodes.begin(), rg_it->second.online_nodes.end(), ecu_node);
                if (ret != rg_it->second.online_nodes.end()) {
                    ALOGI("%s, ECU node [%s] is offline", __func__, dumpECU_Name(ecu_node));
                    // mResourceGroupMx.lock();
                    rg_it->second.online_nodes.erase(ret);
                    // mResourceGroupMx.unlock();
                }
            }
        }
    }

    lock_guard<std::mutex> lk_rg(mResourceGroupMx);
    for (auto rg_it_ = mIplmResourceGroupMap.begin(); rg_it_ != mIplmResourceGroupMap.end(); rg_it_ ++) {
        // Check Resource Group State and notify callback thread to trigger callback.
        previousState = rg_it_->second.state;
        auto member_size = rg_it_->second.members.size();
        auto online_size = rg_it_->second.online_nodes.size();

        if (online_size == 0) {
            currentState = IPLM_RG_State::STATE_NONE;
        } else if (member_size > online_size) {
            currentState = IPLM_RG_State::STATE_PARTIAL;
        } else if (member_size == online_size) {
            currentState = IPLM_RG_State::STATE_FULLY;
        }

        if (currentState != previousState) {
            ALOGI("%s, Resource Group index : %#x [%s], previousState: %#x, currentState: %#x, member_size : %#lx, online_size : %#lx",
                __func__, rg_it_->first, dumpResourceGroup(rg_it_->first), previousState, currentState, member_size, online_size);

            rg_it_->second.state = currentState;
        }

        if ((currentState != IPLM_RG_State::STATE_NONE) && isAllServiceReady()) {
            isIPLinkReady = true;
        } else {
            isIPLinkReady = false;
        }

        if (isIPLinkReady != rg_it_->second.isCanTriggerIPCP) {
            rg_it_->second.isCanTriggerIPCP = isIPLinkReady;

            lock_guard<std::mutex> lk_callback(mCallbackMx);
            for (auto callback_it = rg_it_->second.callbacks.cbegin(); callback_it != rg_it_->second.callbacks.cend(); callback_it ++) {
                Callback_Element element = {
                    .what  = callback_it->second,
                    .index = rg_it_->first,
                    .triggerIPCP = isIPLinkReady,
                };
                mCallbackQueue.emplace(element);
            }
            mCallbackCond.notify_one();
        }

        if (currentState != IPLM_RG_State::STATE_FULLY && (rg_it_->first == IPLM_ResourceGroup_Index::RG_1)) {
            // If Resource Group 1 is not FULLY mode, IPLM module will trigger IPWakeUp VFC automotically.

            if (mVFCRetriggerBulletin > 0)
                mVFCRetriggerBulletin -- ;

            if (mVFCRetriggerBulletin == 0) {
                mVehicleHelper.wakeupVFC();
                mVFCRetriggerBulletin = DEFAULT_VFC_TRIGGER_PERIOD / DEFAULT_IPLM_STEP_PERIOD;
            }
        }
    }

    lock_guard<std::mutex> power_lk(mResourceGroupStateMx);

    // Cancle IPLM_SET_POWER_NORMAL case, it's just for B-CALL.
    // if (mARSFlagSet.any()) {
    //     powerState = IPLM_Power_State::IPLM_SET_POWER_NORMAL;
    // }

    if (mPriorityHighSet.none()) {
        if(mARSFlagSet.none()) {
            powerState = IPLM_Power_State::IPLM_SET_POWER_NULL;
        }
    } else if (mPriorityHighSet.any()) {
        powerState = IPLM_Power_State::IPLM_SET_POWER_HIGH;
    }

    // Update Current Power State to VP
    if (powerState != mPowerState) {
        mPowerState = powerState;
        ALOGI("%s, setPowerState: %d", __func__, mPowerState);
        mVehicleHelper.setPowerState(mPowerState);
    }
}

bool Iplm::isAllServiceReady() {
    // return mPrecfgServicesMap == static_cast<uint32_t> (IPLM_Preconfigured_Service::ALL_READY);

    uint32_t coreServices = static_cast<uint32_t> (IPLM_Preconfigured_Service::Connectivity |
                            IPLM_Preconfigured_Service::OTA |
                            IPLM_Preconfigured_Service::POSITIONING |
                            IPLM_Preconfigured_Service::PrivateLockingInterStorage |
                            IPLM_Preconfigured_Service::RVDC |
                            IPLM_Preconfigured_Service::TransferElectricalPower |
                            IPLM_Preconfigured_Service::ExteriorLightShow |
                            IPLM_Preconfigured_Service::XCall);

    return mPrecfgServicesMap == coreServices;
    // return mPrecfgServicesMap == static_cast<uint32_t> (IPLM_Preconfigured_Service::ALL_READY);

}


void Iplm::iplmStep() {
    int fire_count = DEFAULT_IPLM_ACTIVITY_PERIOD / DEFAULT_IPLM_STEP_PERIOD;

    while (!mIsTerminating) {
        if (isAllServiceReady() && (mIplmActivityBulletin == 0)) {
            broadcastIpActivity();
        }

        mIplmActivityBulletin++;
        if (mIplmActivityBulletin == fire_count) {
            mIplmActivityBulletin = 0;
        }

        updateInternalResourceGroup();
        // 100ms unit
        sleep_for(milliseconds(DEFAULT_IPLM_STEP_PERIOD));
    }
}

void Iplm::doCallback() {
    while (!mIsTerminating) {
        unique_lock<std::mutex> lk(mCallbackMx);
        if (mCallbackQueue.empty()) {
            mCallbackCond.wait(lk);
            continue;
        }

        // ALOGD("%s is ready for handling callback", __func__);
        auto task = mCallbackQueue.front();
        mCallbackQueue.pop();
        lk.unlock();

        ALOGD("%s is triggered for %s", __func__, dumpResourceGroup(task.index));
        sp<IIplmCallback> callback = task.what.promote();
        if (callback != nullptr) {
            callback->onResourceGroupStateChanged(task.index, task.triggerIPCP);
        }
    }
}

/****************  Private Methods ****************/
const char * Iplm::dumpECU_Name(ECU_Node node) {
    switch (node) {
        case ECU_Node::NODE_TCAM:
            return "[TCAM]";
        case ECU_Node::NODE_VGM:
            return "[VGM]";
        case ECU_Node::NODE_ASDM:
            return "[ASDM]";
        case ECU_Node::NODE_DIM:
            return "[DIM]";
        case ECU_Node::NODE_DMM:
            return "[DMM]";
        case ECU_Node::NODE_DVR:
            return "[DVR]";
        case ECU_Node::NODE_PAS:
            return "[PAS]";
        case ECU_Node::NODE_IIM:
            return "[IIM]";
        case ECU_Node::NODE_IHU:
            return "[IHU]";
        default:
            return "unknown";
    }
}

const char * Iplm::dumpPreconfigServiceName(IPLM_Preconfigured_Service service) {
    switch (service) {
        case IPLM_Preconfigured_Service::Connectivity:
            return "[Connectivity]";
        case IPLM_Preconfigured_Service::ExteriorLightShow:
            return "[ExteriorLightShow]";
        case IPLM_Preconfigured_Service::OTA:
            return "[OTA]";
        case IPLM_Preconfigured_Service::POSITIONING:
            return "[POSITIONING]";
        case IPLM_Preconfigured_Service::PrivateLockingInterStorage:
            return "[PrivateLockingInterStorage]";
        case IPLM_Preconfigured_Service::RVDC:
            return "[RVDC]";
        case IPLM_Preconfigured_Service::TrafficJamPilot:
            return "[TrafficJamPilot]";
        case IPLM_Preconfigured_Service::TransferElectricalPower:
            return "[TransferElectricalPower]";
        case IPLM_Preconfigured_Service::XCall:
            return "[X-Call]";
        default:
            return "unknown";
    }
}

const char * Iplm::dumpPreconfigServiceStatus(ServiceStatus status) {
    switch (status) {
        case ServiceStatus::OnLine:
            return "OnLine";
        case ServiceStatus::OffLine:
            return "OffLine";
        default:
            return "unknown";
    }
}

const char * Iplm::dumpResourceGroup(IPLM_ResourceGroup_Index index) {
    switch (index) {
        case IPLM_ResourceGroup_Index::RG_1:
            return "RG_1";
        case IPLM_ResourceGroup_Index::RG_2:
            return "RG_2";
        case IPLM_ResourceGroup_Index::RG_3:
            return "RG_3";
        case IPLM_ResourceGroup_Index::RG_4:
            return "RG_4";
        case IPLM_ResourceGroup_Index::RG_5:
            return "RG_5";
        case IPLM_ResourceGroup_Index::RG_6:
            return "RG_6";
        case IPLM_ResourceGroup_Index::RG_7:
            return "RG_7";
        default:
            return "unknown";
    }
}

void Iplm::handleHidlDeath(CookieType cookie) {
    // Cleanup Death HIDL callbacks
    ALOGD("%s, called", __func__);
    lock_guard<std::mutex> lk(mResourceGroupMx);

    for (auto it = mIplmResourceGroupMap.begin(); it != mIplmResourceGroupMap.end(); it++) {
        ALOGI("%s, %#x", __func__, it->first);
        auto callback_it = it->second.callbacks.find(cookie);
        if (callback_it != it->second.callbacks.end()) {
            callback_it->second->unlinkToDeath(mDeathRecipient);
            it->second.callbacks.erase(callback_it);
        }

        for (auto cookie_it = it->second.cookieMap.begin(); cookie_it != it->second.cookieMap.end(); ) {
            if (cookie_it->second == cookie) {
                it->second.cookieMap.erase(cookie_it ++);
            } else {
                cookie_it ++;
            }
        }
    }
}

void Iplm::convertToBigEndian(iplm_activity_t * packet) {
    uint8_t * p = (uint8_t *) &packet->header;

    uint16_t service_id       = packet->header.service_id;
    uint16_t operation_id     = packet->header.operation_id;
    uint32_t length           = packet->header.length;
    uint32_t sender_handle_id = packet->header.senderHandleId;
    uint8_t  pro_version      = packet->header.protocol_version;
    uint8_t  oper_type        = packet->header.op_type;
    uint8_t  payload_type     = packet->header.payload_type;
    uint8_t  proc_field       = packet->header.proc;

    UINT16_TO_BE_STREAM(p, service_id);
    UINT16_TO_BE_STREAM(p, operation_id);
    UINT32_TO_BE_STREAM(p, length);
    UINT32_TO_BE_STREAM(p, sender_handle_id);
    UINT8_TO_BE_STREAM(p, pro_version);
    UINT8_TO_BE_STREAM(p, oper_type);
    UINT8_TO_BE_STREAM(p, payload_type);
    UINT8_TO_BE_STREAM(p, proc_field);
}

void Iplm::convertToLittleEndian(iplm_activity_t * packet) {
    uint8_t *   unp = (uint8_t *)packet;
    uint16_t    sid;
    uint16_t    op_id;
    uint32_t    len;
    uint32_t    sender_handle_id;
    uint8_t     version;
    uint8_t     op_tp;
    uint8_t     payload_tp;
    uint8_t     proc_flag;

    BE_STREAM_TO_UINT16(sid, unp);
    BE_STREAM_TO_UINT16(op_id, unp);
    BE_STREAM_TO_UINT32(len, unp);
    BE_STREAM_TO_UINT32(sender_handle_id, unp);
    BE_STREAM_TO_UINT8(version, unp);
    BE_STREAM_TO_UINT8(op_tp, unp);
    BE_STREAM_TO_UINT8(payload_tp, unp);
    BE_STREAM_TO_UINT8(proc_flag, unp);

    packet->header.service_id = sid;
    packet->header.operation_id = op_id;
    packet->header.length = len;
    packet->header.senderHandleId = sender_handle_id;
    packet->header.protocol_version = version;
    packet->header.op_type = op_tp;
    packet->header.payload_type = payload_tp;
    packet->header.proc = proc_flag;

    // ALOGI("%s, [sid %#x], [op_id %#x], [len %#x], [sendHandlerId %#x], [op_type %#x], [payload_type %#x]",
    //         __func__,
    //         packet->header.service_id,
    //         packet->header.operation_id,
    //         packet->header.length,
    //         packet->header.senderHandleId,
    //         packet->header.op_type,
    //         packet->header.payload_type);
}


} // namespace implementation
} // namespace V1_0
} // namespace iplm
} // namespace xma
} // namespace ecarx
} // namespace vendor
